/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
/**
* This generates topo MCL path from the PeLoTe data
* Requires that the map_matcher has been run first.
*
* usage: topo_log [dir]
* 
* Processes the laser_update.txt file in the directory [dir]
* uses the topo_odo.txt to obtain the "correct initial pose"
* Saves the topoMCL.txt
*/ 

#define MAP_FILE_NAME "../../../../res/autmap-strip.txt"

//#define NUMBER_OF_RUNS 7
//float particles[NUMBER_OF_RUNS] = {100,200,500,1000,5000,10000,20000};

#define NUMBER_OF_RUNS 1
float particles[NUMBER_OF_RUNS] = {5000};
#include<stdio.h>
#include<stdlib.h>
#include<math.h>

#include "TOccupancyGrid.h" ///< Occupancy grid class
#include "glutImageGUI.h"   ///< For visualisation
#include "CTopoOccuMCL.h"
#include "JPEGEncoder.hpp"						
/// For reading the data from file

int readSilmuPoseData(FILE *f,mcl::scan &meas, mcl::pose &p){
		float t1,t2;
		int i;
		char c;
		fscanf(f,"%f %f %f %f %f %d\n",&t1,&p.x,&p.y,&t2,&p.a,&i); ///< Pose
		meas.N = 0;
		
		return 0;
}

 int readSilmuPoseDataNuppu(FILE *f,mcl::scan &meas, mcl::pose &p){
		float t1,t2,foo1,foo2;
		int i;
		char c;
		//fscanf(f,"%f %f %f %f %f %d %f %f\n",&t1,&p.x,&p.y,&t2,&p.a,&i,&foo1,&foo2); ///< Pose
		fscanf(f,"%f %f %f %f %f %d %f %f\n",&t1,&foo1,&foo2,&t2,&p.a,&i,&p.x,&p.y); ///< Pose
		meas.N = 0;
		
		return 0;
}


int readLaserOdometryData(FILE *f,mcl::scan &meas, mcl::pose &p){
		float p1,p2,p3;
		int i;
		char c;
		fscanf(f,"%f %f %f %f %f %f ", &p.x,&p.y,&p.a,&p1,&p2,&p3); ///< Pose
		for(i=0;i<361;i++){
				fscanf(f,"%f",&meas.r[i]);
				meas.a[i]=0.5*i*M_PI/180.0;
				fscanf(f,"%c",&c);
   // fprintf(stderr,"%.1f ",meas.r[i]);
				if(c=='\n' && i<361){
						fprintf(stderr,"EOL reached at i=%d\n",i);
						meas.N = i;
						return 0;
				}
		}
		meas.N = 361;
		return 0;

}



void setRGBpix(unsigned char *rgb,int i,int j, int Nx, unsigned char r,unsigned char g, unsigned char b ){
	rgb[3*Nx*j+3*i]=r;
	rgb[3*Nx*j+3*i+1]=g;
	rgb[3*Nx*j+3*i+2]=b;
}

void setRGBmap(unsigned char *rgb,TOccupancyGrid *gr, unsigned char r,unsigned char g, unsigned char b){
	float *data = gr->getDataPtr();
	int Nx,Ny;
	gr->getDim(Nx,Ny);
	float v;
	
	for(int j=0;j<Ny;j++){
		for(int i=0;i<Nx;i++){
				v = data[Nx*j+i];
				if(v>0.29) v = 0.9;
				setRGBpix(rgb,i,j, Nx, r*v,g*v,b*v);
		}
	}	
}

void setRGBpixBold(unsigned char *rgb,int i,int j, int Nx, unsigned char r,unsigned char g, unsigned char b){
		for(int ii=i-1;ii<i+1;ii++){
				for(int jj=j-1;jj<j+1;jj++){
						setRGBpix(rgb,ii,jj,Nx,r,g,b);
				}
		}

}

void drawLine(unsigned char *RGB, int Nx,int Ny,float resolution,   ///Grid parameters
							uint pix, uint piy,float angle, float length, ///< Start pixel, angle to draw and linelength
							unsigned char r,unsigned char g,unsigned char b){
		float dy = sin(angle);
		float dx = cos(angle);
		float rr=0;
		int i=0;
		uint x,y;
		while(rr<length){
				x = pix + i*dx;
				y = piy - i*dy;
				RGB[3*Nx*y+3*x] = r;
				RGB[3*Nx*y+3*x+1] = g;
				RGB[3*Nx*y+3*x+2] = b;
				i++;
				rr+=resolution;
		}
}
void saveJPEG(const char *fname, unsigned char *rgb,int Nx, int Ny){
		CJPEGEncoder encoder;
		encoder.SetRGBDataPtr((const char *)rgb,Nx,Ny);
		FILE *f = fopen(fname,"wb");
		fwrite(encoder.GetJPEGDataPtr(), 
					 encoder.GetJPEGDataSize(), 
					 1, f);
		fclose(f);
		
}
int main(int argc, char *argv[]){
		float grid_size = 45.0;
		
		CTopoOccuMCL *topo;
		grid_t *origmap;
		grid_t *dataPtr;
		int Nx,Ny;
		unsigned char *rgb;
		
		
		
		if(argc != 2){
				fprintf(stderr,"\nUsage: ./topo_log [dir]\n\n");
				fprintf(stderr,"[dir] - the directory where the laser_update.txt is located\n");
				return 0;
		}
		char *directory=argv[1];
		
		fprintf(stderr,"Using directory %s\n",directory);
		
		if(chdir(directory)==-1){
				fprintf(stderr,"Invalid directory '%s'!\n",directory);
				return 0;
		}
		
		FILE *fcor = fopen("topo_odo.txt","rt");
		mcl::pose initial_pose;
		fscanf(fcor,"%f %f %f\n",&initial_pose.x,&initial_pose.y,&initial_pose.a);
		fprintf(stderr,"Using initial pose (%.2f %.2f %.2f)\n",initial_pose.x,initial_pose.y,initial_pose.a);
		fclose(fcor);
		
		
		FILE *log = fopen("laser_update.txt","rt");
		if(log==NULL){
				fprintf(stderr,"No such file\n");	
				return 0;
		}
		
		
		//origmap = (grid_t *) malloc(Nx*Ny*sizeof(float));
		//dataPtr = topo->O->getDataPtr();
		//memcpy(origmap,dataPtr,Nx*Ny*sizeof(float));		
		
		topo = new CTopoOccuMCL();
			
		topo->initialize(MAP_FILE_NAME,
										 1.0, 
					 100, 
			initial_pose,  
	 mcl::pose(1.5,1.5,M_PI/30),
						 0.1,
			 1.0);
		topo->O->getDim(Nx,Ny);
		delete topo; topo=NULL;	
		
		
		///Initialize Image display
		rgb = (unsigned char *) malloc(3*Nx*Ny);
		addImageWindow(Nx, Ny);
		glutImageGUI_Start();
		
		mcl::scan s; ///< for occupancy grid
		s.allocate(361);
		mcl::pose odo_cur, odo_ref,mest;
				
		displayImage(0, rgb,Ny,Nx);
		
		int cnt=0;
		
		
		vector<mcl::pose> path;
		vector<mcl::pose> variance;
		vector<mcl::pose> odometry;
		vector< vector<mcl::pose> >	pose_storage;					
		vector< vector<mcl::pose> >	var_storage;					
		float time_vec[NUMBER_OF_RUNS];
		
		
		while(!feof(log)){
				readLaserOdometryData(log,s,odo_cur);
				odometry.push_back(odo_cur);
		}
		
		for(int runs=0;runs<NUMBER_OF_RUNS;runs++){
			
			odo_ref=odometry[0];
			topo = new CTopoOccuMCL();
			
			topo->initialize(MAP_FILE_NAME,
											1.0, 
					 						particles[runs], 
											initial_pose,  
	 										mcl::pose(1,1,M_PI/30),
						 					0.1,
			 								1.0);
			
			long int ss0,us0,ss1,us1,ss2,us2,sec,us,ms;
			float fms;
			ownTime_GetTimeOfDay(&ss0,&us0);
		
			for(int i=1;i<odometry.size();i++){
				odo_cur = odometry[i];
			
				////////////////////////////////////////////////////////////////////////////
				/// The MCL
				////////////////////////////////////////////////////////////////////////////
				mest.setToDifferentialPose( odo_cur,odo_ref);
				topo->runMCL(mest, mcl::pose(fabs(mest.x)*0.3,fabs(mest.y)*0.3,0.001*fabs(mest.a)),  s);
				odo_ref = odo_cur;
				
				mcl::pose mp = topo->filt.getDistributionMean(true); ///< The distribution mean
				mcl::pose var = topo->filt.getDistributionVariances();
				mp.to2PI();
				
				path.push_back(mp);
				variance.push_back(var);
				
				
				//if(cnt%100==0){
				//		fprintf(stderr,"Est:(%.2f,%.2f,%.2f) ",mest.x,mest.y,mest.a);
				//		fprintf(stderr,"VAR: %.2f  %.2f %.2f\n",var.x,var.y,var.a);
				//}
				
				////////////////////////////////////////////////////////////////////////////
				/// Visualisation
				////////////////////////////////////////////////////////////////////////////
				
				
				if(cnt%250==0 || i==odometry.size()-1){
					//fprintf(stdout,"Drawing");
					setRGBmap(rgb,topo->O, 200,200, 220);
					//	setRGBmap(rgb,topo.O, 255,255, 255);
					Grid::TCoord2D coord;
					Grid::TPixel pix;
					for(int i=0;i<topo->filt.NumOfParticles;i++){
						coord.x = topo->filt.Particles[i].x;
						coord.y = topo->filt.Particles[i].y;
						pix = topo->O->toPixel(coord);
						setRGBpix(rgb,pix.x,pix.y, Nx, 255,255, 0);
					}
					
					Grid::TCoord2D ocoord;
					ocoord.x = mp.x;ocoord.y = mp.y;
					pix = topo->O->toPixel(ocoord);
					drawLine(rgb, Nx,Ny, topo->O->getResolution(),pix.x,pix.y,mp.a, 2.0,255,0,0);
					drawLine(rgb, Nx,Ny, topo->O->getResolution(),pix.x,pix.y,mp.a+M_PI/2, 2.0,0,255,0);
					
					for(int i=0;i<path.size();i+=1){
							ocoord.x = path[i].x;ocoord.y = path[i].y;
							pix = topo->O->toPixel(ocoord);
							setRGBpixBold(rgb,pix.x,pix.y, Nx, 255,0, 0 );
					}
					displayImage(0, rgb,Ny,Nx);
					char jpeg_name[100];
					sprintf(jpeg_name,"%d_out_result.jpg");
					saveJPEG(jpeg_name, rgb,Nx,Ny);
					/*
					if(cnt == 0) {
							char c;
							fprintf(stderr,"PRESS ENTER TO CONTINUE\n");
							fscanf(stdin,"%c",&c);
					}*/
					
				}
				////////////////////////////////////////////////////////////////////////////
				/// The end
				////////////////////////////////////////////////////////////////////////////
				
				cnt++;
			}
			
			ownTime_GetTimeOfDay(&ss2,&us2);
			sec = ss2-ss0;us = us2-us0; ms = sec*1000+us/1000;
			fms=(float)sec*1000.0+(float)us/1000.0;
			time_vec[runs] = fms/cnt;
			fprintf(stderr,"TOTAL::(%.4f) = (%.4f)/scan \n",fms,fms/cnt);
			cnt=0;
			pose_storage.push_back(path);
			var_storage.push_back(variance);
			variance.clear();
			path.clear();
			delete topo; topo=NULL;
		}
		
		FILE *topo_info = fopen("topo_info.txt","wt");
		for(int i=0;i<NUMBER_OF_RUNS;i++){
			fprintf(topo_info,"%.1f ",particles[i]);
		}
		fprintf(topo_info,"\n");
		for(int i=0;i<NUMBER_OF_RUNS;i++){
			fprintf(topo_info,"%f ",time_vec[i]);
		}
		fprintf(topo_info,"\n");
		
		fclose(topo_info);
		
		
		fprintf(stderr,"We have %d trajectories, size of which is %d Odo size=%d\n",pose_storage.size(),pose_storage[0].size(),odometry.size());
		
		FILE *topo_filu = fopen("topoMCL_pose.txt","wt");
		for(int j=0;j<pose_storage[0].size();j++){
			
			for(int i=0;i<NUMBER_OF_RUNS;i++){
				fprintf(topo_filu,"%.3f %.3f %.3f ",pose_storage[i][j].x,pose_storage[i][j].y,pose_storage[i][j].a);
			}
			fprintf(topo_filu,"\n");
		}
		fclose(topo_filu);
		
		topo_filu = fopen("topoMCL_var.txt","wt");
		for(int j=0;j<var_storage[0].size();j++){
			
			for(int i=0;i<NUMBER_OF_RUNS;i++){
				fprintf(topo_filu,"%.3f %.3f %.3f ",var_storage[i][j].x,var_storage[i][j].y,var_storage[i][j].a);
			}
			fprintf(topo_filu,"\n");
		}
		fclose(topo_filu);
		
		/*
		fprintf(stderr,"The End - press ctrl C to stop\n");
		while(1){
				setRGBmap(rgb,topo.O, 200,255, 200);
			Grid::TCoord2D coord;
			Grid::TPixel pix;
			
			for(int i=0;i<topo.filt.NumOfParticles;i++){
				coord.x = topo.filt.Particles[i].x;
				coord.y = topo.filt.Particles[i].y;
				pix = topo.O->toPixel(coord);
				setRGBpix(rgb,pix.x,pix.y, Nx, 255,255, 0);
			}
					
			Grid::TCoord2D ocoord;
			
			//for(int i=0;i<path.size();i+=10){
			//	ocoord.x = path[i].x;ocoord.y = path[i].y;
			//	pix = topo.O->toPixel(ocoord);
			//	drawLine(rgb, Nx,Ny, topo.O->getResolution(),pix.x,pix.y,path[i].a, 2.0,255,0,0);
				//drawLine(rgb, Nx,Ny, topo.O->getResolution(),pix.x,pix.y,path[i].a+M_PI/2, 2.0,0,255,0);
			//}
			for(int i=0;i<path.size();i+=1){
				ocoord.x = path[i].x;ocoord.y = path[i].y;
				pix = topo.O->toPixel(ocoord);
				//setRGBpix(rgb,pix.x,pix.y, Nx, 255,0, 0 );
				setRGBpixBold(rgb,pix.x,pix.y, Nx, 255,0, 0 );
			}
			displayImage(0, rgb,Ny,Nx);
			usleep(1000*1000);
		}
		*/
		return 0;
}